﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

namespace NpcFramework.Traffic
{
    public class TrafficCar : MonoBehaviour
    {
        public static Dictionary<Collider,TrafficCar> Col2TrafficCarMap = new Dictionary<Collider, TrafficCar>();

        
        public CarWheelsTransform wheelsTransforms;

        public CarSetting carSetting;
        
        public Transform rayOrigin;

        
        [SerializeField]
        [DisplayOnly]
        private Transform[] _wheels;
        [SerializeField]
        [DisplayOnly]
        private WheelCollider[] _wheelColliders;

        [Range(1,30)]
        public int blockCastFrame = 6;            // 阻挡物检测间隔帧数

        public bool autoDestroy = false;          // 一段时间未行走是否自动销毁
        public float destroyDelay = 60;
        
        private float _timeStopped;

        private Vector3 _shiftCentre = new Vector3(0.0f, -0.05f, 0.0f);
        private Vector3 _steerCurAngle = Vector3.zero;

        // 目标点
        private Vector3 _targetPos;
        
        private Vector3 _relativeVector;

        private float _steer = 0.0f;
        //private float _distance;
        private float _speed;
        private float _brake = 0;
        private float _motorTorque = 0;
        
        private Rigidbody _myRigidbody;

        public Collider RigidCollider;

        private bool enableBlockCast = true;
        
        private void Start()
        {
            _myRigidbody = GetComponent<Rigidbody>();
            _myRigidbody.centerOfMass = _shiftCentre;
            
            Col2TrafficCarMap.Add(RigidCollider,this);
        }

        private void FixedUpdate()
        {
            Moving();
        }


        private void OnDestroy()
        {
            Col2TrafficCarMap.Remove(RigidCollider);
        }

        public float GetHeight()
        {
            if (_wheelColliders.Length > 0)
            {
                return _wheelColliders[0].radius - _wheelColliders[0].transform.localPosition.y;
            }

            return 0f;
        }
        
        public void SetTargetPos(Vector3 nextPos)
        {
            _targetPos = nextPos;
        }

        private int _blockFrame = 0;
        private void Moving()
        {
            _relativeVector = transform.InverseTransformPoint(_targetPos);
            _steer = ((_relativeVector.x / _relativeVector.magnitude) * carSetting.maxSteerAngle);

            _speed = _myRigidbody.velocity.magnitude * carSetting.speedFactor;
            rayOrigin.localRotation = Quaternion.Euler(new Vector3(0, _steer, 0));

            
            _blockFrame++;
            //if (_blockFrame > blockCastFrame)
            {
                _brake = BlockCast();
                _blockFrame = 0;
            }


            if (_speed < 1f)
            {
                if (autoDestroy)
                {
                    _timeStopped += Time.fixedTime;
                    if (_timeStopped > destroyDelay)
                    {
                        Destroy(transform.gameObject);
                    }
                }
            }
            else
            {
                _timeStopped = 0f;
            }
            
            
            
            float bk = 0;

            Quaternion _rot;
            Vector3 _pos;

            for (int k = 0; k < 4; k++)
            {
                if (_speed > carSetting.limitSpeed)
                {
                    bk = Mathf.Lerp(100, 1000, (_speed - carSetting.limitSpeed) / 10);
                }

                if (bk > _brake)
                {
                    _brake = bk;
                }


                if (_brake == 0)
                {
                    _wheelColliders[k].brakeTorque = 0;
                }
                else
                {
                    _wheelColliders[k].motorTorque = 0;
                    _wheelColliders[k].brakeTorque = carSetting.brakePower * _brake;
                }
                

                if (k < 2)
                {
                    _motorTorque = Mathf.Lerp(carSetting.carPower * 30, 0, _speed / carSetting.limitSpeed);
                    _wheelColliders[k].motorTorque = _motorTorque;
                    _wheelColliders[k].steerAngle = _steer;
                }

                _wheelColliders[k].GetWorldPose(out _pos, out _rot);
                _wheels[k].position = _pos;
                _wheels[k].rotation = _rot;
            }


            if (wheelsTransforms.backRight2)
            {
                wheelsTransforms.backRight2.rotation = wheelsTransforms.backRight.rotation;
                wheelsTransforms.backLeft2.rotation = wheelsTransforms.backRight.rotation;
            }

            //steeringwheel movement
            if (carSetting.carSteer)
                carSetting.carSteer.localEulerAngles = new Vector3(_steerCurAngle.x, _steerCurAngle.y, _steerCurAngle.z - _steer);
            
        }

        
        public void SwitchBlockCast(bool enable)
        {
            enableBlockCast = enable;
        }
        
        // 射线检测阻挡物
        private float BlockCast()
        {
            if (!enableBlockCast)
            {
                return 0;
            }
            
            float rStop = 0;


            return rStop;
//            RaycastHit hit;
//            int wdist = 10;
//            float rStop = 0;
//            rayOrigin.localRotation = Quaternion.Euler(new Vector3(0, _steer, 0));
//            Debug.DrawRay(rayOrigin.position, rayOrigin.forward * wdist, Color.yellow);
//
//            if (Physics.Raycast(rayOrigin.position, rayOrigin.forward, out hit, wdist))
//            {
//                if (hit.collider != RigidCollider && !hit.collider.isTrigger)
//                {
//                    Debug.DrawRay(rayOrigin.position, rayOrigin.forward * wdist, Color.red);
//                    rStop = 6000 / hit.distance;
//                }
//            }
//            return rStop;
        }
        

#if UNITY_EDITOR

        public void GenerateConfigure()
        {
            
            if (!wheelsTransforms.frontRight)
                wheelsTransforms.frontRight = GetTransformWheel("FrontRight_Wheel");

            if (!wheelsTransforms.frontLeft)
                wheelsTransforms.frontLeft = GetTransformWheel("FrontLeft_Wheel");

            if (!wheelsTransforms.backRight)
                wheelsTransforms.backRight = GetTransformWheel("BackRight_Wheel");

            if (!wheelsTransforms.backLeft)
                wheelsTransforms.backLeft = GetTransformWheel("BackLeft_Wheel");

            if (!wheelsTransforms.backRight2)
                wheelsTransforms.backRight2 = transform.Find("BackRight2_Wheel");

            if (!wheelsTransforms.backLeft2)
                wheelsTransforms.backLeft2 = transform.Find("BackLeft2_Wheel");

            
            // 设置刚体
            Rigidbody rigidbody = transform.GetComponent<Rigidbody>();
            if (!rigidbody)
            {
                rigidbody = transform.gameObject.AddComponent<Rigidbody>();
            }

            if (rigidbody.mass < 4000f)
            {
                rigidbody.mass = 4000f;
            }
            
            
            // 射线原点
            rayOrigin = transform.Find("RayOrigin");
            if (!rayOrigin)
            {
                rayOrigin = new GameObject("RayOrigin").transform;
                rayOrigin.SetParent(transform);
                rayOrigin.localRotation= Quaternion.identity;
                float p = wheelsTransforms.frontRight.localPosition.z + 0.6f;
                rayOrigin.localPosition = new Vector3(0f, 0.5f, p);
            }

            
            // 计算最大转向角度
            carSetting.maxSteerAngle = (int)Mathf.Clamp(
                Vector3.Distance(wheelsTransforms.frontRight.transform.position, wheelsTransforms.backRight.transform.position) * 12,
                35, 
                72);

            
            
            // 生成 WheelCollider
            
            _wheels = new Transform[4];
            _wheelColliders = new WheelCollider[4];
            
            GameObject center = new GameObject("Center");
            Vector3 nCenter = new Vector3(0, 0, 0);
            
            _wheels[0] = wheelsTransforms.frontRight;
            _wheels[1] = wheelsTransforms.frontLeft;
            _wheels[2] = wheelsTransforms.backRight;
            _wheels[3] = wheelsTransforms.backLeft;
            for(int i = 0; i < 4; i++)
            {
                _wheelColliders[i] = SetWheelComponent(i);
                // Define CenterOfMass
                center.transform.SetParent(_wheels[i].transform);
                center.transform.localPosition = new Vector3(0, 0, 0);
                center.transform.SetParent(transform);
                center.transform.localPosition -= new Vector3(0, _wheelColliders[i].radius, 0);
                nCenter += center.transform.localPosition;

            }
            _shiftCentre = (nCenter / 4);
            DestroyImmediate(center);
        }
        
        
        private Transform GetTransformWheel(string wheelName)
        {
            var wheelTrans = GameObject.FindObjectsOfType(
                typeof(GameObject)).Select(g => g as GameObject).Where(g => g.name.Equals(wheelName) && g.transform.parent.root == transform).ToArray();

            if (wheelTrans.Length > 0)
            {
                return wheelTrans[0].transform;
            }
            return null;
        }

        private WheelCollider SetWheelComponent(int index)
        {
            WheelCollider result;

            if (transform.Find(_wheels[index].name + " - WheelCollider"))
            {
                DestroyImmediate(transform.Find(_wheels[index].name + " - WheelCollider").gameObject);
            }

            GameObject wheelCol = new GameObject(_wheels[index].name + " - WheelCollider");

            wheelCol.transform.SetParent(transform);
            wheelCol.transform.position = _wheels[index].position;
            wheelCol.transform.eulerAngles = transform.eulerAngles;

            WheelCollider col = (WheelCollider)wheelCol.AddComponent(typeof(WheelCollider));

            result = wheelCol.GetComponent<WheelCollider>();

            // 轮胎参数设置
            JointSpring js = col.suspensionSpring;
            js.spring = carSetting.springs;
            js.damper = carSetting.dampers;
            col.suspensionSpring = js;

            col.suspensionDistance = 0.05f;
            col.radius = (_wheels[index].GetComponent<MeshFilter>().sharedMesh.bounds.size.z * _wheels[index].transform.localScale.z) * 0.5f;
            col.mass = 1500;

            return result;
        }


        private void OnDrawGizmos()
        {
            //Gizmos.color = Color.green;
            //Gizmos.DrawCube(_targetPos,new Vector3(1,10,1));
            
        }

#endif
        
        
    }
}
